#include "Linux_Config.h"
#include "TSS.h"

//用于控制简单电机速度的TSS_Pid库的示例用法
//想法是通过改变PWM施加的电压来控制电机的速度

#define POINT 100
//从传感器读取电机速度的功能
float readSensor(void)
{
    //本函数需要用户自己实现
    //这里只是做简单的示例!
    //模拟传感器读数波动的工况!
    //数值在50-150波动
    srand(time(NULL));
    static float sensor = 0;
    if (sensor < 0.90 * POINT)
    {
        sensor += 0.01 * POINT;
    }
    else if (sensor > 1.10 * POINT)
    {
        sensor -= 0.01 * POINT;
    }
    else
    {
        sensor = (80 + rand() % (120 - 80 + 1)) / 100.0 * POINT;
    }
    // Log_printf("Read", "%.2f\n", sensor);
    Log_printf(NULL, "%.2f, ", sensor);
    return sensor;
}
//从电位计读取所需速度的功能
float readSetPoint(void)
{
    //本函数需要用户自己实现
    //这里只是做简单的示例!
    return POINT;
}
//设置PWM输出以控制电机的功能
void setActuator(float output)
{
    //本函数需要用户自己实现
    //这里只是做简单的示例!
    // Log_printf("Out", "%.2f\n", output);
    Log_printf(NULL, "%.2f\n", output);
}

TSS_PidDef controller = {0};
//任务示例
uint8_t PID_TASK_ID = 0;
void Pid_Task(TSS_TaskMessageDef *p)
{
    float setPoint = 0.0; //需要调节到的值
    float sensor = 0.0;   //传感器读数
    float output = 0.0;   //电机输出

    sensor = readSensor();     // 更新传感器度读数
    setPoint = readSetPoint(); // 更新用户所需调节到的值

    // 更新pid并产生一个新的输出
    float terms[3];
    output = TSS_PidProcess_(&controller, setPoint, sensor, terms);
    // Log_printf("Pid", "Up + Ui + Ud = %.2f+%.2f+%.2f = %.2f\n",
    //            terms[0], terms[1], terms[2], terms[0] + terms[1] + terms[2]);
    Log_printf(NULL, "%.2f, %.2f, %.2f, %.2f, ",
               terms[0], terms[1], terms[2], terms[0] + terms[1] + terms[2]);
    // 更新控制执行器, 由输出控制电机转速
    setActuator(output);
}

/**
  * @brief  pid操作的api示例
  * @retval 无
  * @note   无
  */
int main()
{
    TSS_Init();
    Systick_Init();
    // Log_printf("INFO", "Systerm Init ok!\n");

    TSS_PidInit(&controller);

    // 设置1秒内调节几次
    controller.Ts = 10;
    // 设置输出范围
    controller.OutputMax = 100.0;
    controller.OutputMin = -100.0;
    // 设置调整常量
    controller.K = 0.5;  //比例
    controller.Ti = 500; //积分
    controller.Td = 1.0; //微分

    /* Add your application code here */
    PID_TASK_ID = TSS_ThreadCreate(Pid_Task, 1, TASK_HZ, 100);

    /* Start systerm here */
    TSS_Start();
    while (1)
    {
    }

    return 0;
}
